/**
 * @file main.cpp
 * @brief ROS2 IMU传感器节点主程序
 * @author AI进化论-花生
 * @date 2024
 * @version 0.0.0
 * @license MIT
 * 
 * 本文件实现了RobotOne项目的传感器节点，提供IMU数据发布功能。
 * 遵循现代C++17标准和ROS2最佳实践。
 */

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include <chrono>
#include <memory>
#include <string>

using namespace std::chrono_literals;

/**
 * @class SensorNode
 * @brief 传感器节点类，负责IMU数据的采集和发布
 * 
 * 该类继承自rclcpp::Node，实现了完整的传感器节点功能：
 * - 定时数据发布
 * - 错误处理和恢复
 * - 参数配置管理
 * - 日志记录
 */
class SensorNode : public rclcpp::Node
{
public:
    /**
     * @brief 构造函数，初始化传感器节点
     * 
     * 初始化ROS2节点，创建发布者和定时器，设置参数。
     * 使用现代C++特性确保资源安全和异常处理。
     */
    SensorNode() : Node("sensor_node")
    {
        try {
            // 声明节点参数
            this->declare_parameter("publish_rate", 1.0);
            this->declare_parameter("queue_size", 10);
            this->declare_parameter("frame_id", "imu_link");
            
            // 获取参数值
            double publish_rate = this->get_parameter("publish_rate").as_double();
            int queue_size = this->get_parameter("queue_size").as_int();
            std::string frame_id = this->get_parameter("frame_id").as_string();
            
            // 创建发布者，发布到 "sensor_data" 主题
            publisher_ = this->create_publisher<std_msgs::msg::String>(
                "sensor_data", 
                static_cast<size_t>(queue_size)
            );
            
            // 创建定时器，根据参数设置发布频率
            auto timer_period = std::chrono::milliseconds(
                static_cast<int>(1000.0 / publish_rate)
            );
            timer_ = this->create_wall_timer(
                timer_period, 
                std::bind(&SensorNode::timer_callback, this)
            );
            
            // 初始化消息计数器
            message_count_ = 0;
            
            RCLCPP_INFO(this->get_logger(), 
                "Sensor node started successfully!");
            RCLCPP_INFO(this->get_logger(), 
                "Publish rate: %.1f Hz, Queue size: %d, Frame ID: %s", 
                publish_rate, queue_size, frame_id.c_str());
                
        } catch (const std::exception& e) {
            RCLCPP_ERROR(this->get_logger(), 
                "Failed to initialize sensor node: %s", e.what());
            throw;
        }
    }

    /**
     * @brief 析构函数，清理资源
     */
    ~SensorNode() noexcept override
    {
        RCLCPP_INFO(this->get_logger(), 
            "Sensor node shutting down. Total messages published: %lu", 
            static_cast<unsigned long>(message_count_.load()));
    }

private:
    /**
     * @brief 定时器回调函数，发布传感器数据
     * 
     * 在定时器触发时调用，创建并发布传感器数据消息。
     * 包含错误处理和数据验证。
     */
    void timer_callback()
    {
        try {
            // 检查发布者是否有效
            if (!publisher_) {
                RCLCPP_ERROR(this->get_logger(), 
                    "Publisher is not initialized!");
                return;
            }
            
            // 创建消息
            auto message = std_msgs::msg::String();
            message.data = "Hello World from sensor node! Message #" + 
                          std::to_string(++message_count_);
            
            // 发布消息
            publisher_->publish(message);
            
            // 记录日志（每10条消息记录一次，避免日志过多）
            if (message_count_.load() % 10 == 0) {
                RCLCPP_INFO(this->get_logger(), 
                    "Published message #%lu: '%s'", 
                    static_cast<unsigned long>(message_count_.load()), message.data.c_str());
            }
            
        } catch (const std::exception& e) {
            RCLCPP_ERROR(this->get_logger(), 
                "Error in timer callback: %s", e.what());
        }
    }
    
    // 成员变量
    rclcpp::TimerBase::SharedPtr timer_;                                    ///< 定时器指针
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;        ///< 发布者指针
    std::atomic<uint64_t> message_count_{0};                               ///< 消息计数器
};

/**
 * @brief 主函数，程序入口点
 * 
 * 初始化ROS2系统，创建传感器节点，并开始事件循环。
 * 包含完整的异常处理和资源管理。
 * 
 * @param argc 命令行参数个数
 * @param argv 命令行参数数组
 * @return int 程序退出码，0表示成功
 */
int main(int argc, char** argv)
{
    try {
        // 初始化ROS2
        rclcpp::init(argc, argv);
        
        // 创建传感器节点
        auto node = std::make_shared<SensorNode>();
        
        RCLCPP_INFO(node->get_logger(), 
            "Starting RobotOne sensor node...");
        
        // 开始事件循环
        rclcpp::spin(node);
        
        // 清理ROS2
        rclcpp::shutdown();
        
        RCLCPP_INFO(rclcpp::get_logger("main"), 
            "Sensor node stopped successfully.");
        
        return 0;
        
    } catch (const std::exception& e) {
        RCLCPP_ERROR(rclcpp::get_logger("main"), 
            "Fatal error in main: %s", e.what());
        return 1;
    } catch (...) {
        RCLCPP_ERROR(rclcpp::get_logger("main"), 
            "Unknown fatal error in main");
        return 1;
    }
}